# Create the curve instance
import numpy as np
from matplotlib import pyplot as plt

from utils.b_spline_geomdl import approximate_b_spline_path as geomdl_b_spline
from utils.b_spline_scipy import approximate_b_spline_path as scipy_b_spline, b_spline_path_test

way_point_x = [-28.079998016357422, -21.060001373291016, -15.47999382019043, -9.539995193481445, 1.980010986328125,
               9.539995193481445, 19.259994506835938, 30.59999656677246, 15.480010986328125, 5.219999313354492,
               -11.879997253417969, -24.1199951171875, -30.95999526977539, -28.079998016357422]
way_point_y = [66.42001342773438, 70.0199966430664, 68.03999328613281, 67.1399917602539, 68.03999328613281,
               68.57998657226562, 69.83999633789062, 63.53999328613281, 61.20000076293945, 58.86000061035156,
               61.739990234375, 64.260009765625, 65.34001159667969, 66.42001342773438]


def run_b_spline():
    print("start!!")
    # way_point_x = [-1.0, 3.0, 4.0, 2.0, 1.0]
    # way_point_y = [0.0, -3.0, 1.0, 1.0, 3.0]
    n_course_point = len(way_point_x) * 5  # sampling number

    rax, ray = scipy_b_spline(way_point_x, way_point_y, n_course_point, s=0, degree=3)

    rax_g, ray_g = geomdl_b_spline(way_point_x, way_point_y, degree=3, count=n_course_point)

    # show results
    plt.plot(way_point_x, way_point_y, '-og', label="way points")
    plt.plot(rax, ray, '-r', label="Scipy B-Spline path")
    plt.plot(rax_g, ray_g, '-b', label="Geomdl B-Spline path")
    plt.grid(True)
    plt.suptitle("Scipy B-Spline path")
    plt.legend()
    plt.axis("equal")
    plt.show()



if __name__ == '__main__':
    run_b_spline()
    # b_spline_path_test()
